/***************************************************************************
 *   Copyright (C) 2006 by Archimedes Projekt   *
 *   thelsb@gmail.com   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "mservcontmreader.h"

using namespace TM;

MServConTMReader::MServConTMReader(const QString &sPort,QObject *parent): MDataReader(sPort, parent)
{
}


MServConTMReader::~MServConTMReader()
{
}

/*!
    \fn MServConTMReader::readTM()
    Pure virtual implementation: Reads the ServCon telemetry from the serial port
    (used in MTelemetry)
 */
MDataBlock* MServConTMReader::readTM() throw(MException)
{
  MDataBlock *block = NULL;
  MDataBlock::TYPE type = MDataBlock::ERR;
  sync();
   while(m_serial->bytesAvailable() <  sizeof(MDataBlock))
   {
    msleep(100);
   }
    qDebug()<<"CamCon block";
  if(m_serial->bytesAvailable() >= sizeof(MDataBlock))
  {
   //qDebug()<<"before allocation"<<" bytes:"<<m_serial->bytesAvailable()<<"sizeof:"<<sizeof(MDataBlock);
   block = new MDataBlock(MDataBlock::SERVCON);
   //qDebug()<<"after allocation";
   m_serial->readData((char*)block,sizeof(MDataBlock));
   qDebug()<<(int)block->blockNum();
   msleep(1);
  } 
  qDebug()<<"returning";
  return block; 
}



/*!
    \fn TM::MServConTMReader::sync()
 */
void TM::MServConTMReader::sync()
{
    unsigned char temp=0;
    int i;
    qDebug()<<"syncing...";
    while(1)
    {
     i=0;
     if(m_serial->bytesAvailable()>4)
       if( m_serial->getChar((char*)&temp) && temp==0xa5 )
       {
        //qDebug("-%x",temp);
        for(i=0;i<(SYNC_SIZE-1) && temp == 0xa5;++i)
        {
         m_serial->getChar((char*)&temp);
         msleep(10);
         if(temp !=0xa5) break;
         //qDebug("--%x i:%d",temp,i);
        }
       }
     //else  qDebug("---%x",temp);
     if(i==(SYNC_SIZE-1) ) break;
     msleep(100);
    }
}
